#include <cmath>
#include <iostream>
#include <list>
#include <boost/numeric/mtl/mtl.hpp>

#include "fmm.h"
#include "isoline.h"
#include "backtracking.h"


void generate_merged_mesh_geodesics(Mesh & mesh, std::list<Mesh::Point> & path)
{
	unsigned n_vertex = mesh.n_vertices();
	
	for (unsigned i = 0; i < n_vertex; ++i)
	{
		std::cout<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
	}
	
	for (std::list<Mesh::Point>::iterator it = path.begin(); it!=path.end(); ++it)
	{
		std::cout<<"v "<<(*it)<<std::endl;
	}
	
	for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
	{
		std::cout<<"f ";
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			std::cout<<fv_it.handle().idx() + 1<<' ';
		}
		std::cout<<std::endl;
	}

	for (unsigned i = 0; i < path.size() - 1;++i)
	{	
		std::cout<<"f "<<n_vertex+ i + 1 <<' '<<n_vertex+ i + 2 << std::endl;
	}
	
}

void generate_seperate_mesh_geodesics(Mesh & mesh, std::list<Mesh::Point> & path, std::ostream & ostr)
{
	unsigned n_vertex = mesh.n_vertices();
	ostr<<"o model"<<std::endl;
	for (unsigned i = 0; i < n_vertex; ++i)
	{
		ostr<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
	}
	
	ostr<<"s off"<<std::endl;
	for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
	{
		ostr<<"f ";
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			ostr<<fv_it.handle().idx() + 1<<' ';
		}
		ostr<<std::endl;
	}

	ostr<<"o geodesics"<<std::endl;
	for (std::list<Mesh::Point>::iterator it = path.begin(); it!=path.end(); ++it)
	{
		ostr<<"v "<<(*it)<<std::endl;
	}
	ostr<<"s off"<<std::endl;
	for (int i = 0; i < int(path.size()) - 1;++i)
	{	
		ostr<<"f "<<n_vertex+ i + 1 <<' '<<n_vertex+ i + 2 << std::endl;
	}
	
}
void generate_seperate_group_geodesics(Mesh & mesh, std::list<std::list<Mesh::Point> > & path_groups, std::ostream & ostr)
{
	unsigned n_vertex = mesh.n_vertices();
	ostr<<"o model"<<std::endl;
	for (unsigned i = 0; i < n_vertex; ++i)
	{
		ostr<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
	}
	
	ostr<<"s off"<<std::endl;
	for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
	{
		ostr<<"f ";
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			ostr<<fv_it.handle().idx() + 1<<' ';
		}
		ostr<<std::endl;
	}

	unsigned int geo_count = 0;
	for (std::list<std::list<Mesh::Point> >::iterator ll_it = path_groups.begin(); ll_it!=path_groups.end(); ++ll_it)
	{
		ostr<<"o geodesics"<<(geo_count++)<<std::endl;
		for (std::list<Mesh::Point>::iterator l_it = ll_it->begin(); l_it!=ll_it->end(); ++l_it)
		{
			ostr<<"v "<<(*l_it)<<std::endl;
		}
		ostr<<"s off"<<std::endl;
		for (unsigned i = 0; i < ll_it->size() - 1; ++i)
		{
			ostr<<"f "<<n_vertex+ i + 1 <<' '<<n_vertex+ i + 2 << std::endl;
		}
		n_vertex +=ll_it->size();
	}
}

void generate_seperate_mesh_point_cloud(Mesh & mesh, std::list<Mesh::Point> & path)
{
	unsigned n_vertex = mesh.n_vertices();
	std::cout<<"o model"<<std::endl;
	for (unsigned i = 0; i < n_vertex; ++i)
	{
		std::cout<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
	}
	
	std::cout<<"s off"<<std::endl;
	for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
	{
		std::cout<<"f ";
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			std::cout<<fv_it.handle().idx() + 1<<' ';
		}
		std::cout<<std::endl;
	}

	std::cout<<"o geodesics"<<std::endl;
	for (std::list<Mesh::Point>::iterator it = path.begin(); it!=path.end(); ++it)
	{
		std::cout<<"v "<<(*it)<<std::endl;
	}
	std::cout<<"s off"<<std::endl;
	for (int i = 0; i < int(path.size()) ;++i)
	{	
		std::cout<<"f "<<n_vertex+ i + 1 << std::endl;
	}
	
}

